#include "WorldModel.h"

#include "../include/instantiate.h"
#include "pw_exception.h"
#include "UniverseModel.h"

namespace physicsworks {

using exception::IBadArg;
using exception::IBodyAnotherWorld;
using exception::IBodyAlreadyAdded;
using exception::IUnkownBody;

WorldModel::WorldModel(const vec3& a, const vec3& b, SpatialPartitioning* sp)
: name("anonymous"), spatialPartitioning(sp)
{
	
}

void WorldModel::add(Body* b)
{
	typedef UniverseModel::WorldModelHSet WorldModelHSet;
	
	RigidBody* rb = dynamic_cast<RigidBody*>(b);
	Assert<IBadArg>(rb, "Invalid body argument");
	
	
	Assert<IBodyAlreadyAdded>(rigidBodies.find(rb) == rigidBodies.end(),
	                          std::string("The body ") + rb->getName() + " was already added in this world");

	// whether the universe is alive, we must check if the body
	// was already added in another world
	if (universeAlive()) {
		UniverseModel* u = dynamic_cast<UniverseModel*>( instanceUniverse() );
		
		const WorldModelHSet w = u->getWorldsListHSet();
		WorldModelHSet::const_iterator witer;
	
		for (witer = w.begin(); witer != w.end(); ++witer) {
			RigidBodyHSet::const_iterator iter = (*witer)->rigidBodies.find( rb );
		
			// don't add the body whether there exists in another world
			Assert<IBodyAnotherWorld>(iter == (*witer)->rigidBodies.end(), std::string("The body ") + b->getName()
			                                                              + " was already added in the world "
			                                                              + (*witer)->getName());
		}
	}
	
	rigidBodies.insert(rb);
	spatialPartitioning->add(rb);
}

void WorldModel::remove(Body* b)
{
	RigidBody* rb = dynamic_cast<RigidBody*>(b);
	Assert<IBadArg>(rb, "Invalid body argument");
	
	size_t s = rigidBodies.erase(rb);
	Assert<IUnkownBody>(s != 0, std::string("The body ") + rb->getName() + " was never added");
	spatialPartitioning->remove(rb);
}

Body** WorldModel::getBodiesList(int* size)
{
	*size = rigidBodies.size();
	if (*size == 0)
		return 0;
	
	Body** b = new Body*[*size];
	
	RigidBodyHSet::const_iterator iter;
	int i = 0;
	for (iter = rigidBodies.begin(); iter != rigidBodies.end(); ++iter) {
		b[i++] = *iter;
	}
	return b;
}

}
